import random
import subprocess

eep = [0,0,0,0]
sumErr = 0
prevErr = 0
xSet = 0
P = 0
I = 0
D = 0
outpt = []
inpt = []
indx = 0

SAT_MAX = int("0x1FFF",16)
SAT_MIN = -int("0x2000",16)
MULT_CNST = int("0x800",16)

def main():
	genStim()
	runTB()
	getOutpt()
	checkOutpt()


def genStim():
	global inpt, eep
	print('%generating stimulus')
	f = open('accel_vals.data', 'w')
	for i in range(100):
		a = random.randint(SAT_MIN, SAT_MAX)
		inpt.append(a)
		f.write('@' + intToHex(i)[2:] + ' ' + intToHex(a)[2:].zfill(4) + '\n')
		#f.write(intToHex(a)[2:].zfill(4) + '\n')
	f.close()
	f = open('eep_init.data', 'w')
	a = random.randint(SAT_MIN, SAT_MAX)
	f.write('@0 ' + intToHex(a)[2:].zfill(4) + '\n')
	eep[0] = a
	a = random.randint(0, SAT_MAX)
	f.write('@1 ' + intToHex(a)[2:].zfill(4) + '\n')
	eep[1] = a
	a = random.randint(0, SAT_MAX)
	f.write('@2 ' + intToHex(a)[2:].zfill(4) + '\n')
	eep[2] = a
	a = random.randint(SAT_MIN/(2**6), 0)
	f.write('@3 ' + intToHex(a)[2:].zfill(4) + '\n')
	eep[3] = a
	f.close()

def runTB():
	print('%running test bench')
	subprocess.call('vsim -c -novopt -do \'run -all\' work.cbc_dig_tb', shell=True)


def getOutpt():
	global outpt
	print('%getting Output')
	f = open('testOutput.data', 'r')
	for line in f:
		outpt.append(line)
	f.close()

def checkOutpt():
	global indx
	print('%checking Output')
	initVars()
	for arg in outpt:
		if arg[0] == '/' or 'xxxx' in arg:
			pass
		elif arg[0] == '%':
			processCmd(arg)
		else:
			checkCalc(inpt[indx], arg)
			indx += indx


def processCmd(arg):
	print('processing cmd: '+ arg)



def checkCalc(xMeas, arg):
	global sumErr, prevErr
	print 'checking calculation: xMeas: {0}, outpt: {1}'.format(xMeas,arg)
	err = sat(xMeas - xSet)
	duty = sat(P*err)/MULT_CNST
	sumErr = sat(sumErr + err)
	duty = sat(duty+ sat(I*sumErr)/MULT_CNST)
	dErr = sat(err - prevErr)
	duty = sat(duty + sat(D * dErr)/MULT_CNST)
	prevErr = err
	if duty != hexToInt(arg):
		msg = 'Error at inpt: '+ intToHex(xMeas)
		msg += ' Expected: ' + intToHex(duty)
		print msg
	else:
		print 'success: ' + intToHex(duty)


def intToHex(arg):
	if arg < 0:
		arg += 2**14
	return hex(arg)

def hexToInt(arg):
	s = int(arg,16)
	if s >= 2**13:
		s -= 2**14
	return s

def sat(s):
	if(s > SAT_MAX):
		s = SAT_MAX
	if(s < SAT_MIN):
		s = SAT_MIN
	return s


def initVars():
	global sumErr, prevErr, xSet
	global P, I, D, indx
	sumErr = 0
	prevErr = 0
	xSet = eep[0]
	P = eep[1]
	I = eep[2]
	D = eep[3]
	indx = 0



if __name__ == "__main__":
	main()
